#include "render.h"
#include "camera.h"
#include "body.h"
#include "viewer.h"

void Viewer::AddViewer(Viewer* child) {
  std::unique_ptr<Viewer> ptr(child);
  child_views_.push_back(std::move(ptr));
}

RootViewer* RootViewer::Build(const std::string& name, ImageWriter* writer) {
  auto viewer = new RootViewer;
  viewer->name_ = name;
  viewer->writer_ = std::unique_ptr<ImageWriter>(writer);
  return viewer;
}

void RootViewer::UpdateViewer() {
  cv::Mat image;
  for (auto view : child_views_) {
    view->Update(image);
  }

  if (writer_) writer_->WriteImage(image);
  
  cv::imshow(name_, image);
}

ImageViewer* ImageViewer::Build(Camera* cam) {
  auto viewer = new ImageViewer;
  viewer->cam_ = cam;
  return viewer;
}

void ImageViewer::Update(cv::Mat& image) {
  image = cam_->image();

  for (auto view : child_views_) {
    view->Update(image);
  }
}

NormalViewer* NormalViewer::Build(Render* render) {
  auto viewer = new NormalViewer;
  viewer->render_ptr = render;
  return viewer;
}

cv::Mat AlphaBlend(
  const cv::Mat &camera_image,
  const cv::Mat &renderer_image, 
  float opacity) 
{
  cv::Mat image{camera_image.size(), CV_8UC3};
  int v, u;
  const cv::Vec3b *ptr_camera_image;
  const cv::Vec4b *ptr_renderer_image;
  cv::Vec3b *ptr_image;
  const uchar *val_camera_image;
  const uchar *val_renderer_image;
  uchar *val_image;
  float alpha;
  float alpha_scale = opacity / 255.0f;

  for (v = 0; v < camera_image.rows; ++v) {
    ptr_camera_image = camera_image.ptr<cv::Vec3b>(v);
    ptr_renderer_image = renderer_image.ptr<cv::Vec4b>(v);
    ptr_image = image.ptr<cv::Vec3b>(v);
    for (u = 0; u < camera_image.cols; ++u) {
      val_camera_image = ptr_camera_image[u].val;
      val_renderer_image = ptr_renderer_image[u].val;
      val_image = ptr_image[u].val;

      alpha = float(val_renderer_image[3]) * alpha_scale;
      float alpha_inv = 1.0f - alpha;
      val_image[0] =
          char(val_camera_image[0] * alpha_inv + val_renderer_image[0] * alpha);
      val_image[1] =
          char(val_camera_image[1] * alpha_inv + val_renderer_image[1] * alpha);
      val_image[2] =
          char(val_camera_image[2] * alpha_inv + val_renderer_image[2] * alpha);
    }
  }
  return image;
}

void NormalViewer::Update(cv::Mat& image) {
  render_ptr->RenderNormal();
  render_ptr->FetchNormalImage();
  cv::Mat normal = render_ptr->normal_image();
  image = AlphaBlend(image, normal, 0.5f);
  
  for (auto view : child_views_) {
    view->Update(image);
  }
}

AxisViewer* AxisViewer::Build(Camera* camera, Body* body) {
	auto viewer = new AxisViewer;
	viewer->camera_ptr = camera;
	viewer->body_ptr = body;
	return viewer;
}

void AxisViewer::DrawAxes(cv::Mat& image) {
  auto k = camera_ptr->intrinsics();
  auto pose = body_ptr->body2world_pose().matrix();

  auto project = [&](float x, float y, float z) {
    Eigen::Vector4f pt(x, y, z, 1.f);
    pt = pose * pt;
    return cv::Point(pt(0) * k.fx / pt(2) + k.cx, pt(1) * k.fy / pt(2) + k.cy);
  };

  //float rad = body_ptr->maximum_body_diameter();
  float rad = 0.1f;
  float alen = rad / 2.5f;
  cv::Point ao = project(0, 0, 0);
  cv::Point ax = project(alen, 0, 0);
  cv::Point ay = project(0, alen, 0);
  cv::Point az = project(0, 0, alen);

  cv::line(image, ao, ax, cv::Scalar(0, 0, 255), 2);
  cv::line(image, ao, ay, cv::Scalar(0, 255, 0), 2);
  cv::line(image, ao, az, cv::Scalar(255, 0, 0), 2);
}

void AxisViewer::Update(cv::Mat& image) {
  DrawAxes(image);

  for (auto view : child_views_) {
    view->Update(image);
  }
}